/***************************************************************************\
 * Copyright (C) by Keio University
 * DepthSenseSensorDriver.cpp created in 03 2013.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * DepthSenseSensorDriver.cpp is part of HVRL Engine Library.
 *
 * The HVRL Engine Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/sensor/DepthSenseSensorDriver.hpp"
#include "hvrl/common/Common.hpp"

namespace hvrl {

DepthSenseSensorDriver::DepthSenseSensorDriver(void) :
		numberofdevices(0), initialized(false) {
}

DepthSenseSensorDriver::~DepthSenseSensorDriver(void) {
	if (this->initialized == true){
		this->context.stopnodes();
	}
}

bool DepthSenseSensorDriver::refresh(void) {


	if (this->initialized == false) {
		try{
			this->context = DepthSense::Context::create();
		}catch(DepthSense::Exception e){
			Log::add().error("DepthSenseSensorDriver::DepthSenseSensorDriver",e.getMessage());
			return false;
		}
		this->initialized = true;
	}

	try{
		std::vector<DepthSense::Device> da = this->context.getDevices();
	}catch(DepthSense::Exception e){
		Log::add().error("DepthSenseSensorDriver::refresh",e.getMessage());
		return false;
	}

	this->numberofdevices = da.size();

	this->deviceslist.clear();

	/*
	 * Count the number of available OpenNI devices
	 */
	unsigned int i = 0;
	for (std::vector<DepthSense::Device>::iterator it = da.Begin(); it != da.End();
			++it, ++i) {
		DepthSense::Device::Model m = it->getModel();
		this->deviceslist.push_back(DepthSense::Device::Model_toString(m) + " [" + m.getSerialNumber() + "]");
		if(i==0){
			this->drivername = std::string("SoftKinetic");
		}
	}

	this->context.startNodes();
	this->context.run();

	return true;
}

}
